/** 
 * CAS BACnet Stack server example 
 * http://bacnetstack.com/
 * 
 * Note: If you get an error about missing the "BACnetAPI.h" please contact us
 * 1-866-383-1657, Email: info001@chipkin.com
 */ 

/*********************************************************************************
 *  Version, Date,       User, Notes
 *    2.02	 17 Jan 2013  SWS  Created Version table 
 *    2.03   03 Jul 2013  SWS  Updated for latest version of the CAS BACnet API 
 *    2.04   16 Apr 2014  SWS  Added Object list example 
 * 
 ********************************************************************************/

#include "stdafx.h"
#include <conio.h>
#include <stdio.h>
#include <time.h>

#include "SimpleUDP.h"
#include "BACnetAPI.h"
#include "BACnetPrimitiveObjectIdentifier.h"

#define BACNET_PORT					47808 
#define MAX_IP_ADDRESS_LEN			100
#define MAX_XML_LENGTH				1024*20
#define SEND_MESSAGE_FREQUENCY		3 

// This is the device we will interacting with. 
// The default settings should match up with the BACnet server example. 
#define DEVICE_ID					389002
#define DEVICE_NETWORK				0
#define DEVICE_IPADDRESS			"192.168.1.39"

#define WRITE_OBJECT_ID				3
#define WRITE_OBJECT_TYPE			BACnetObjectType::analog_output 
#define WRITE_OBJECT2_ID			4
#define WRITE_OBJECT2_TYPE			BACnetObjectType::analog_output 
#define WRITE_PRIORITY				8 

#define SUBSCRIBE_OBJECT_ID			3333
#define SUBSCRIBE_OBJECT_TYPE		BACnetObjectType::analog_input

#define READ_OBJECT_ID				WRITE_OBJECT_ID
#define READ_OBJECT_TYPE			WRITE_OBJECT_TYPE 

#define WHOIS_LOW_LIMIT				0
#define WHOIS_HIGH_LIMIT			0x3FFFFF

#define REMOTE_NETWORK				1

#define WHOHAS_NAME					"device (389002)" 




using namespace CASBACnetAPI; 
CSimpleUDP						udp ; // Networking 
CBACnetAPI						BACnetAPI;
CBACnetAPIResponce				BACnetResponce ; 
BACnetPrimitiveObjectIdentifier ObjectIdentifier; 
float fWriteValue = 100.0f;

bool DoInput	( ); 
bool SendMessage( BYTE * buffer, USHORT length, bool broadcast = false ); 
USHORT GenerateNPDU( BYTE * buffer, USHORT maxLength, UINT deviceID, USHORT deviceNetwork = 0 , bool expectingReply = false );

void ProcessBACnetMessage( BYTE * buffer, USHORT length, BYTE * fromIPAddress ); 
void PrintHelp(); 

void SendWhoIsMessage						( ) ;
void SendWhoHasMessage						( ) ; 
void SendReadPropertyMessage				( USHORT objectType, UINT objectID, UINT deviceID, USHORT propertyID ); 
void SendReadPropertyMultipleMessage		( USHORT objectType, UINT objectID, UINT deviceID, USHORT propertyID ); 
void SendReadPropertyMultipleCustomMessage	( USHORT objectType, UINT objectID, UINT deviceID); 
void SendWritePropertyMessage				( USHORT objectType, UINT objectID, UINT deviceID, float value); 
void SendWritePropertyMultipleMessage		( ); 
void SendSubscribeCOVMessage				( USHORT objectType, UINT objectID, UINT deviceID ); 
void SendWhoIsRouterToNetwork				( USHORT network) ;
void SendForeignDeviceRegistration			( ); 
void SendReadPropertyMessageObjectList		( UINT deviceID );

void PrintMessageAsXML( BYTE * buffer, USHORT length ); 

void PrintHelp() {
	// Display the help for this example. 
	printf( "\n" );
	printf( "\tI - Send WhoIs Message\n" );
	printf( "\tH - Send WhoHas Message\n" );
	printf( "\tR - Send Read Property Message\n" );
	printf( "\tA - Send Read Property Multiple Message:all\n" );
	printf( "\tM - Send Read Property Multiple Message\n" );
	printf( "\tW - Send Write Property Message\n" );
	printf( "\tE - Send Write Property Multiple Message\n" );
	printf( "\tS - Send Subscribe COV Message\n" ) ;
	printf( "\tN - Send Who-Is-Router-To-Network: 1\n" ) ;
	printf( "\tF - Send Foreign Device Registration \n" ) ;
	printf( "\tO - Send Read Property Object list \n" ) ;

	printf( "\tC - Print Object Count\n" ) ;
	printf( "\tD - Debug \n" ) ;
	printf( "\tQ - Quit\n" );
}

int _tmain(int argc, _TCHAR* argv[])
{
	printf( "Starting up \n" ); 
	printf( "CAS BACnet Stack v%d.%d - Client\n", BACnetAPI.GetMajorVersion(), BACnetAPI.GetMinorVersion() ); 

	PrintHelp(); 

	// In this example we will be connecting to a BACnet IP network using UDP.
	if( ! udp.Connect( BACNET_PORT ) ) {
		printf( "Error. Could not bind to the BACnet UDP port. \nIs there anther version of this applcation running?" ); 
		return 0;
	}

	// Setup the API to allow for certain functionality
	// ------------------------------------------------------------------------	
	// Enable the following two services to be able to make COV subscriptions and process incoming COV Notifications.
	ServerOptions options; 
	options[ protocolServicesSupported_unconfirmedCOVNotification ] = TRUE ;
	options[ protocolServicesSupported_confirmedCOVNotification   ] = TRUE ;
	BACnetAPI.Initialize( options ) ;

	// Set up some buffers to hold the BACnet messages
	int		buffer_length = 0 ;
	BYTE	buffer[ MAXIMUM_BACNET_MESSAGE ] ;
	memset( buffer, 0 , MAXIMUM_BACNET_MESSAGE ); 

	BYTE	fromIPAddress[ MAX_IP_ADDRESS_LEN ] ; 
	memset( fromIPAddress, 0 , MAX_IP_ADDRESS_LEN ); 
	std::string ipaddress = DEVICE_IPADDRESS ;
		
	// Loop until user quits. 
	bool done = false ; 
	while ( ! done ) 
	{
		// Get a message from the UDP socket on the BACnet port. 
		buffer_length = udp.GetMessage( (char *) buffer, MAXIMUM_BACNET_MESSAGE, (char *) fromIPAddress ) ; 
		if( buffer_length > 0 ) {
			// Got a message on our BACnet port
			printf( "FYI. Got message, from [%s], size [%d]\n", fromIPAddress, buffer_length  );
			ProcessBACnetMessage( buffer, buffer_length, fromIPAddress ); 
		}

		if( ! DoInput( ) ) {
			// The user is closing down the system. Quit 
			done = true ; 			
		}

		// Debug 
		// This will check for memory leaks, this value should not change over time. 
		static long oldObjectCount = 0; 
		if( oldObjectCount != CBACnetBase::GetObjectCount() ) {
			printf( "Object Count %d (%d)\n", CBACnetBase::GetObjectCount(), CBACnetBase::GetObjectCount() - oldObjectCount );
			oldObjectCount = CBACnetBase::GetObjectCount() ; 
			printf( "\n\n" );
		}
				
		// Be nice and give some time back to other process. 
		Sleep( 0 ); 		
	}

	// We are all done. Shutdown the system. 
	printf( "Shutting down. \n" ); 
	return 0;
}

void PrintMessageAsXML( BYTE * buffer, USHORT length ) {

	// Set up some buffers for the XML Messages 
	char	xml_buffer[ MAX_XML_LENGTH ] ;
	memset( xml_buffer, 0 , MAX_XML_LENGTH); 

	CBACnetAPIResponce	BACnetMessage ; 
	// Decode this message
	if( BACnetMessage.Decode( buffer+4, length-4 ) <= 0  ) {			
		// Error could not decode message. 
		printf( "Can not decode the message for XML\n\n" );
		return ; 
	}	

	// Display the decoded message as XML for reference 
	if( BACnetMessage.ToXML(xml_buffer, MAX_XML_LENGTH) > 0 ) {
		printf( "Message: \n%s\n\n", xml_buffer ); 
	}
	// Everything looks okay 
}

void ProcessBACnetMessage( BYTE * buffer, USHORT buffer_length, BYTE * fromIPAddress ) 
{
	printf("Before ProcessBACnetMessage, BACnet Objects=[%d]\n", CBACnetBase::GetObjectCount() );

	// Decode this message
	if( BACnetResponce.Decode( buffer+4, buffer_length-4 ) <= 0  ) {			
		// Error could not decode message. 
		printf( "Can not decode the message\n\n" );
		return ; 
	}

	printf( "Incomming: \n" ); 
	PrintMessageAsXML( buffer, buffer_length ); 

	// Check the response 
	unsigned char pdutype = BACnetResponce.GetPDUType() ; 
	switch( pdutype ) 
	{
		// Read property responses. 
		case BACnetPDU::complexACK :
		{
			CBACnetComplexAckPDU * pComplexAckPDU =  BACnetResponce.GetPDUComplexAck();
			if( pComplexAckPDU != NULL ) {
				if( pComplexAckPDU->service_ACK_choice == BACnetServiceAckChoice::readProperty ) {
					BACnetReadPropertyAck * pReadPropertyAck = ( BACnetReadPropertyAck *) pComplexAckPDU->service_ACK ;
					if( pReadPropertyAck != NULL ) {
						if( pReadPropertyAck->isValid() ) {

							printf( "ReadProperty. originalInvokeID=[%d] ObjectType=[%d], ObjectInstance=[%d], PropertyIdentifier=[%d] ",
								pComplexAckPDU->original_invokeID, 
								pReadPropertyAck->objectIdentifier.GetObjectType(),
								pReadPropertyAck->objectIdentifier.GetInstanceNumber(),
								pReadPropertyAck->propertyIdentifier.Get() ); 

							BYTE dataType = pReadPropertyAck->propertyValue.GetType() ; 

							// Depending on the data type print the value 
							if( dataType == BACnetApplicationDatatypes::Real ) {
								BACnetPrimitiveREAL * pPrimitiveREAL = pReadPropertyAck->propertyValue.GetBACnetPrimitiveREAL() ; 
								if( pPrimitiveREAL != NULL ) {
									printf( "value=Float[%f]", pPrimitiveREAL->Get() ); 
								}
							} else {
								// This is not a float, Lets just print any other type as a string. 
								char valueBuffer[1024]; 
								if( pReadPropertyAck->propertyValue.ToString(valueBuffer, 1024 ) > 0 ) {
									printf( "value=Str[%s]", valueBuffer ); 
								}
							}
							printf( "\n" ); 
							printf( "\n" ); 
						}
					}
				} // End of BACnetServiceAckChoice::readProperty 
				else if( pComplexAckPDU->service_ACK_choice == BACnetServiceAckChoice::readPropertyMultiple ) {

					printf( "ReadPropertyMultiple - originalInvokeID=[%d] ", pComplexAckPDU->original_invokeID ); 

					BACnetReadPropertyConditionalAck * pReadPropertyConditionalAck = ( BACnetReadPropertyConditionalAck *) pComplexAckPDU->service_ACK ;
					if( pReadPropertyConditionalAck != NULL ) {
						for( size_t offsetReadAccessResult = 0 ; offsetReadAccessResult < pReadPropertyConditionalAck->GetCount() ; offsetReadAccessResult++ ) {

							BACnetReadAccessResult * pReadAccessResult = pReadPropertyConditionalAck->Get( offsetReadAccessResult ) ; 
							if( pReadAccessResult == NULL ) {
								continue ; 
							}

							BACnetPrimitiveObjectIdentifier	* pObjectIdentifier = pReadAccessResult->GetObjectIdentifier(); 
							if( pObjectIdentifier == NULL ) {
								continue; 
							}

							char objectIdentifierName[1024];
							pObjectIdentifier->ToString( objectIdentifierName, 40 ); 
							printf( "Object: %s\n", objectIdentifierName ); 


							for( size_t offsetReadAccessResultStack = 0 ; offsetReadAccessResultStack < pReadAccessResult->GetCount() ; offsetReadAccessResultStack++ )
							{
								BACnetReadAccessResultStack * pReadAccessResultStack = pReadAccessResult->Get( offsetReadAccessResultStack ) ; 
								if( pReadAccessResultStack == NULL ) {
									continue; 
								}

								char propertyIdentifierName[1024];
								pReadAccessResultStack->propertyIdentifier.ToString( propertyIdentifierName, 1024 ); 
								printf( "\tProperty: %s\n", propertyIdentifierName );
											
								switch( pReadAccessResultStack->readResult ){
									case BACnetReadAccessResultStack::error:
									{
										char readAccessResultStackErrorName[1024]; 
										pReadAccessResultStack->propertyAccessError.ToString( readAccessResultStackErrorName, 1024 ) ;
										printf( "\t\tError: %s\n", readAccessResultStackErrorName );

										break; 
									}
									case BACnetReadAccessResultStack::value:
									{
										char readAccessResultStackValueName[1024*5]; 
										pReadAccessResultStack->propertyValue.ToString( readAccessResultStackValueName, 1024 *5 );
										printf( "\t\tValue: %s\n", readAccessResultStackValueName );
										break; 
									}
								} // End of switch( pReadAccessResultStack->readResult )
							} // End of for loop offsetReadAccessResultStack 
						}
					}
				} // End of BACnetServiceAckChoice::readPropertyMultiple
			}
			break; 
		} // End of case BACnetPDU::complexACK :

		// I-AM, Who-IS, Who-Has, UnconfirmedCOVNotification, ect.. 
		case BACnetPDU::unconfirmed_request :
		{
			CBACnetUnconfirmedRequestPDU * pUnconfirmedRequestPDU =  BACnetResponce.GetPDUUnconfirmedRequest();
			if( pUnconfirmedRequestPDU != NULL ) {
				unsigned char serviceChoice = pUnconfirmedRequestPDU->GetServiceChoice() ; 

				if( serviceChoice == BACnetUnconfirmedServiceChoice::i_Am ) {
					BACnetIAmRequest * pIAmRequest = (BACnetIAmRequest*) pUnconfirmedRequestPDU->GetServiceRequest() ; 
					if( pIAmRequest != NULL ) {
						printf( "IAM from, ObjectType=[%d], ObjectInstance=[%d], VendorID=[%d]\n",
							pIAmRequest->GetIAmDeviceIdentifier()->GetObjectType(), 
							pIAmRequest->GetIAmDeviceIdentifier()->GetInstanceNumber(), 
							pIAmRequest->GetVendorID()); 
					}
				} // End of BACnetUnconfirmedServiceChoice::i_Am
							
				else if( serviceChoice == BACnetUnconfirmedServiceChoice::i_Have )  
				{
					BACnetIHaveRequest * pIHaveRequest = (BACnetIHaveRequest*) pUnconfirmedRequestPDU->GetServiceRequest() ; 
					if( pIHaveRequest != NULL ) {
						printf( "IHave from,  DeviceType=[%d], DeviceInstance=[%d], ObjectType=[%d], ObjectInstance=[%d],  name=[%s]\n", 
							pIHaveRequest->GetDeviceIdentifier()->GetObjectType(),
							pIHaveRequest->GetDeviceIdentifier()->GetInstanceNumber(),
							pIHaveRequest->GetObjectIdentifier()->GetObjectType(),
							pIHaveRequest->GetObjectIdentifier()->GetInstanceNumber(),
							pIHaveRequest->GetObjectName()->Get().c_str() ); 
					}			

				} // end of BACnetUnconfirmedServiceChoice::i_Have

				else if( serviceChoice == BACnetUnconfirmedServiceChoice::unconfirmedCOVNotification )
				{
					BACnetUnconfirmedCOVNotificationRequest * pUnconfirmedCOVNotificationRequest = ( BACnetUnconfirmedCOVNotificationRequest * ) pUnconfirmedRequestPDU->GetServiceRequest() ;
					if( pUnconfirmedCOVNotificationRequest != NULL ) {
									
						printf( "UnconfirmedCOVNotification from, DeviceInstance=[%d], ObjectType=[%d], ObjectInstance=[%d], ", 
							pUnconfirmedCOVNotificationRequest->GetInitiatingDeviceIdentifier(),
							pUnconfirmedCOVNotificationRequest->GetMonitoredObjectIdentifier()->GetObjectType(),
							pUnconfirmedCOVNotificationRequest->GetMonitoredObjectIdentifier()->GetInstanceNumber() ) ;

						// COV Notifications contain a list of properties whose values have changed.
						// They will usually have Present_Value and Status_Flags
						// Here we cycle through the properties and get their value.
						for( size_t i = 0 ; i < pUnconfirmedCOVNotificationRequest->GetNumberOfValues() ; i++ ) {
										
							// In this example, we will only get the present value that was changed.
							if( pUnconfirmedCOVNotificationRequest->GetPropertyValue( i )->GetPropertyIdentifier() == BACnetPropertyIdentifier::present_value ) {
								printf( "Property=[%d], ", pUnconfirmedCOVNotificationRequest->GetPropertyValue( i )->GetPropertyIdentifier() ) ;

								BACnetPropertyValue * pPropertyValue = pUnconfirmedCOVNotificationRequest->GetPropertyValue( i ) ; 
								if( pPropertyValue == NULL ) {
									printf( "Undefined Object Type\n" ) ;
									continue; 
								}
								BACnetAbstractSyntaxType * pAbstractSyntaxType = pPropertyValue->GetValue() ; 
								if( pAbstractSyntaxType == NULL ) {
									printf( "Undefined Object Type\n" ) ;
									continue; 
								}

								// Depending on what object type, the present value will have a different datatype
								switch( pUnconfirmedCOVNotificationRequest->GetMonitoredObjectIdentifier()->GetObjectType() )
								{
									case BACnetObjectType::analog_input:
									case BACnetObjectType::analog_output:
									case BACnetObjectType::analog_value:
									{
										
										printf( "Value=[%f]\n", pAbstractSyntaxType->GetBACnetPrimitiveREAL()->Get() ) ;
										break ;
									}

									case BACnetObjectType::binary_input:
									case BACnetObjectType::binary_output:
									case BACnetObjectType::binary_value:
									case BACnetObjectType::multi_state_input:
									case BACnetObjectType::multi_state_output:
									case BACnetObjectType::multi_state_value:
									{
										printf( "Value=[%d]\n", pAbstractSyntaxType->GetBACnetPrimitiveENUM()->Get() ) ;
										break ;
									}
									default:
									{
										printf( "Undefined Object Type\n" ) ;
										break ;
									}
								}
							}
						}
					}
				} // end of BACnetUnconfirmedServiceChoice::unconfirmedCOVNotification
			}

			break;
		} // end of BACnetPDU::unconfirmed_request

		// ConfirmedCOVNotification
		case BACnetPDU::confirmed_request:
		{
			CBACnetConfirmedRequestPDU * pConfirmedRequestPDU = BACnetResponce.GetPDUConfirmedRequest() ;
			if( pConfirmedRequestPDU != NULL ) {

				if( pConfirmedRequestPDU->GetServiceChoice() == BACnetConfirmedServiceChoice::confirmedCOVNotification ) {
					BACnetConfirmedCOVNotificationRequest * pConfirmedCOVNotificationRequest = ( BACnetConfirmedCOVNotificationRequest * ) pConfirmedRequestPDU->GetServiceRequest() ;
					if( pConfirmedCOVNotificationRequest != NULL ) {
									
						printf( "ConfirmedCOVNotification from, DeviceInstance=[%d], ObjectType=[%d], ObjectInstance=[%d], ", 
							pConfirmedCOVNotificationRequest->GetInitiatingDeviceIdentifier(),
							pConfirmedCOVNotificationRequest->GetMonitoredObjectIdentifier()->GetObjectType(),
							pConfirmedCOVNotificationRequest->GetMonitoredObjectIdentifier()->GetInstanceNumber() ) ;

						// COV Notifications contain a list of properties whose values have changed.
						// They will usually have Present_Value and Status_Flags
						// Here we cycle through the properties and get their value.
						for( size_t i = 0 ; i < pConfirmedCOVNotificationRequest->GetNumberOfValues() ; i++ ) {
										
							// In this example, we will only get the present value that was changed.
							if( pConfirmedCOVNotificationRequest->GetPropertyValue( i )->GetPropertyIdentifier() == BACnetPropertyIdentifier::present_value ) {
								printf( "Property=[%d], ", pConfirmedCOVNotificationRequest->GetPropertyValue( i )->GetPropertyIdentifier() ) ;

								BACnetPropertyValue * pPropertyValue = pConfirmedCOVNotificationRequest->GetPropertyValue( i ) ; 
								if( pPropertyValue == NULL ) {
									printf( "Undefined Object Type\n" ) ;
									continue; 
								}
								BACnetAbstractSyntaxType * pAbstractSyntaxType = pPropertyValue->GetValue() ; 
								if( pAbstractSyntaxType == NULL ) {
									printf( "Undefined Object Type\n" ) ;
									continue; 
								}


								// Depending on what object type, the present value will have a different datatype
								switch( pConfirmedCOVNotificationRequest->GetMonitoredObjectIdentifier()->GetObjectType() )
								{
									case BACnetObjectType::analog_input:
									case BACnetObjectType::analog_output:
									case BACnetObjectType::analog_value:
									{
										printf( "Value=[%f]\n",pAbstractSyntaxType->GetBACnetPrimitiveREAL()->Get() ) ;
										break ;
									}

									case BACnetObjectType::binary_input:
									case BACnetObjectType::binary_output:
									case BACnetObjectType::binary_value:
									case BACnetObjectType::multi_state_input:
									case BACnetObjectType::multi_state_output:
									case BACnetObjectType::multi_state_value:
									{
										printf( "Value=[%d]\n", pAbstractSyntaxType->GetBACnetPrimitiveENUM()->Get() ) ;
										break ;
									}
									default:
									{
										printf( "Undefined Object Type\n" ) ;
										break ;
									}
								}
							}
						}

						// Now we have to ack that the ConfirmedCOVNotification was received
						printf( "Simple Ack message\n" ) ;

						UINT ret = 0 ;
						buffer_length = 4 ;	// Start 4 bytes in to the message to give room for the BVLL 
									
						// Generate the NPDU 
						ret = GenerateNPDU( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length, DEVICE_ID, DEVICE_NETWORK, true );
						if( ret <= 0 ) {
							printf( "Error: Could not generate the NPDU request" ); 
							return; 
						}
						buffer_length += ret ;

						// Generate the simple Ack message. 
						ret = BACnetAPI.SimpleAck (
												buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length,
												BACnetResponce.GetInvokeID(), BACnetConfirmedServiceChoice::confirmedCOVNotification ) ;
															
						if( ret <= 0 ) {
							printf( "Error: Could not generate the WriteProperty request" ); 
							return; 
						}
						buffer_length += ret ; 

						// if there is a message to send, send it
						if( buffer_length > 0 ) { 

							if( ! SendMessage( buffer, buffer_length ) ) {
								return ; 
							}
							buffer_length = 0 ; 
						}
					}
				} // end of BACnetConfirmedServiceChoice::ConfirmedCOVNotification
			}

			break ;
		} // end of BACnetPDU::confirmed_request

		// SimpleAck (in response to SubscribeCOV or WriteProperty)
		case BACnetPDU::simpleACK :
		{
			CBACnetSimpleAckPDU * pSimpleAckPDU = BACnetResponce.GetPDUSimpleAck() ;
			if( pSimpleAckPDU != NULL ) {

							
				if( pSimpleAckPDU->GetServiceAck() == BACnetServiceAckChoice::writeProperty ) {
					printf( "WriteProperty, acked\n" ) ;
				} // end of BACnetSimpleAck for WriteProperty

				else if( pSimpleAckPDU->GetServiceAck() == BACnetServiceAckChoice::subscribeCOV ) {
					printf( "SubscribeCOV, acked\n" ) ;
				} // end of BACnetSimpleAck for SubscribeCOV

				printf( "\tOriginalInvokeID = %d\n", pSimpleAckPDU->GetOriginalInvokeID() ); 
			}

			break ;
		} // end of BACnetPDU::simpleAck


		case BACnetPDU::error :
		{
			CBACnetErrorPDU * pErrorPDU  = BACnetResponce.GetPDUError(); 
			if( pErrorPDU != NULL ) {
				BACnetError * pError = pErrorPDU->GetError(); 
				if( pError != NULL ) {
					printf( "BACnet Error: original_invokeID=[%d], error_choice=[%d], error_class=[%d], error_code=[%d]\n", pErrorPDU->GetOriginalInvokeID(), pErrorPDU->GetErrorChoice(), pError->GetErrorClass(), pError->GetErrorCode() );

					char errorBuffer[ MAX_XML_LENGTH ] ; 
					pError->ToString( errorBuffer, MAX_XML_LENGTH ); 
					printf( "BACnet Error:  %s\n", errorBuffer ); 
				}
			}

			break; 
		} // end of BACnetPDU::error

		case BACnetPDU::abort :
		{
			CBACnetAbortPDU * pAbortPDU  = BACnetResponce.GetPDUAbort(); 
			if( pAbortPDU != NULL ) {
				printf( "BACnet Abort: original_invokeID=[%d], reason=[%d]\n", pAbortPDU->GetOriginalInvokeID(), pAbortPDU->GetAbortReason() );
			}

			break; 
		} // end of BACnetPDU::abort

		case BACnetPDU::reject :
		{
			CBACnetRejectPDU * pRejectPDU  = BACnetResponce.GetPDUReject(); 
			if( pRejectPDU != NULL ) {
				printf( "BACnet Reject: original_invokeID=[%d], reason=[%d]\n", pRejectPDU->GetOriginalInvokeID(), pRejectPDU->GetRejectReason() );
				char errorBuffer[ MAX_XML_LENGTH ] ; 
				pRejectPDU->ToString( errorBuffer, MAX_XML_LENGTH ); 
				printf( "BACnet Reject:  %s\n", errorBuffer ); 
			}

			break; 
		} // end of BACnetPDU::abort

	} // End of switch( pdutype ) 

	printf("Before BACnetResponce.Reset() BACnet Objects=[%d]\n", CBACnetBase::GetObjectCount() );
	BACnetResponce.Reset(); 
	printf("After BACnetResponce.Reset() BACnet Objects=[%d]\n", CBACnetBase::GetObjectCount() );

	printf("After ProcessBACnetMessage, BACnet Objects=[%d]\n", CBACnetBase::GetObjectCount() );
}


bool SendMessage( BYTE * buffer, USHORT length, bool broadcast /* = false */ )
{
	if( length <= 0 || buffer == NULL ) {
		return false; 
	}

	printf("Before PrintMessageAsXML BACnet Objects=[%d]\n", CBACnetBase::GetObjectCount() );

	printf( "Outgoing: \n" ); 
	PrintMessageAsXML( buffer, length ); 

	printf("After  PrintMessageAsXML BACnet Objects=[%d]\n", CBACnetBase::GetObjectCount() );


	std::string ipAddress; 

	// Add the BVLL header for BACnet IP messages 
	BACnetVirtualLinkLayer bvll ;
	if( broadcast ) {
		ipAddress = "192.168.1.255";
		bvll.SetOriginalBroadcastNPDU( length ); 		
	} else { 
		bvll.SetOriginalUnicastNPDU( length ); 
	
		ipAddress = DEVICE_IPADDRESS ;
	}
	bvll.Encode( buffer, 4 ) ;

	// Send the message 
	printf( "Sending request, to [%s], size [%d]\n\n", DEVICE_IPADDRESS, length );
	USHORT ret = udp.SendMessage( ipAddress.c_str(), BACNET_PORT, buffer, length ); 
	if( ret <= 0 ) {
		return false; 
	}	

	return true ; 
}


bool DoInput( )
{
	// Check for user input 
	if( ! _kbhit() ) {
		return true; 
	}
	char action = toupper( _getch() ) ; 

	if( action == 'Q' ) {
		return false; 
	}

	// Send WhoIs Message. 
	else if( action == 'I' ) {
		SendWhoIsMessage( ); 
	}

	// Send WhoHas Message. 
	else if( action == 'H' ) {
		SendWhoHasMessage( ); 
	}

	// Send ReadProperty Message. 
	else if( action == 'R' ) {		
		SendReadPropertyMessage( READ_OBJECT_TYPE, READ_OBJECT_ID, DEVICE_ID, BACnetPropertyIdentifier::present_value ); 
	}

	// Send ReadPropertyMultiple:All Message. 
	else if( action == 'A' ) {		
		SendReadPropertyMultipleMessage( READ_OBJECT_TYPE, READ_OBJECT_ID, DEVICE_ID, BACnetPropertyIdentifier::all ); 
	}

	// Send ReadPropertyMultiple Custom Message. 
	else if( action == 'M' ) {		
		SendReadPropertyMultipleCustomMessage( READ_OBJECT_TYPE, READ_OBJECT_ID, DEVICE_ID ); 
	}

	// Send Write Property Message. 
	else if( action == 'W' ) {		
		fWriteValue += 1.2f;
		SendWritePropertyMessage( WRITE_OBJECT_TYPE, WRITE_OBJECT_ID, DEVICE_ID, fWriteValue ); 
	}
	else if( action == 'E' ) {		
		fWriteValue += 1.2f;
		SendWritePropertyMultipleMessage( ); 
	}

	// Send Subscribe COV message. 
	else if( action == 'S' ) {	
		SendSubscribeCOVMessage( SUBSCRIBE_OBJECT_TYPE, SUBSCRIBE_OBJECT_ID, DEVICE_ID ); 
	}

	// Send Subscribe COV message. 
	else if( action == 'N' ) {	
		SendWhoIsRouterToNetwork( REMOTE_NETWORK ); 
	}

	// Send Foreign Device Registration
	else if( action == 'F' ) {	
		SendForeignDeviceRegistration( ); 
	}

	else if( action == 'O' ) {	
		SendReadPropertyMessageObjectList( DEVICE_ID ); 
	}
	else if( action == 'C' ) {	
		printf( "Object Count %d\n", CBACnetBase::GetObjectCount() );
	}

	// Send Foreign Device Registration
	else if( action == 'D' ) {	
		unsigned char buffer[100];
		unsigned short buffer_length = 0 ; 
		buffer[buffer_length++] = 0x01; 
		buffer[buffer_length++] = 0x04; 
		buffer[buffer_length++] = 0x00; 
		buffer[buffer_length++] = 0x03; 
		buffer[buffer_length++] = 0xb6; 
		buffer[buffer_length++] = 0x0c; 
		buffer[buffer_length++] = 0x0c; 
		buffer[buffer_length++] = 0x00; 
		buffer[buffer_length++] = 0x00; 
		buffer[buffer_length++] = 0x00; 
		buffer[buffer_length++] = 0x01; 
		buffer[buffer_length++] = 0x19; 
		buffer[buffer_length++] = 0x55; //  b9 44 ff

		
		// Decode this message
		if( BACnetResponce.Decode( buffer, buffer_length ) <= 0  ) {			
			// Error could not decode message. 
			printf( "Can not decode the message\n\n" );
			return true; 
		}

		printf( "Responce: \n" ); 
		PrintMessageAsXML( buffer, buffer_length ); 

	}
	else {
		PrintHelp(); 
	}
	
	return true; 
}




void SendWhoIsMessage( ) 
{
	printf( "\n\n----------------------------------------------\n");
	printf( "Sending a WhoIs message\n"); 

	int		buffer_length = 0 ;
	BYTE	buffer[ MAXIMUM_BACNET_MESSAGE ] ;
	memset( buffer, 0 , MAXIMUM_BACNET_MESSAGE ); 

	UINT ret = 0 ; 
	buffer_length = 4 ; // Start 4 bytes in to the message to give room for the BVLL 

	// Generate the NPDU 
	// This is a broadcast message and does not need to be directed at any specific device. 
	ret = GenerateNPDU( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length, 0xFFFF );
	// ret = BACnetAPI.CreateNPDU ( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length, 0xFFFF );
	if( ret <= 0 ) {
		printf( "Error: Could not generate the NPDU request" ); 
		return; 
	}
	buffer_length += ret ; 

	ret = BACnetAPI.WhoIs( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length, WHOIS_LOW_LIMIT, WHOIS_HIGH_LIMIT ) ;
	if( ret <= 0 ) {
		printf( "Error: Could not generate the WhoIs request" ); 
		return; 
	}
	buffer_length += ret ; 

	SendMessage( buffer, buffer_length, true) ;
}


void SendWhoHasMessage() {
	printf( "\n\n----------------------------------------------\n");
	printf( "WhoHas message\n"); 

	int		buffer_length = 0 ;
	BYTE	buffer[ MAXIMUM_BACNET_MESSAGE ] ;
	memset( buffer, 0 , MAXIMUM_BACNET_MESSAGE ); 

	UINT ret = 0 ; 
	buffer_length = 4 ; // Start 4 bytes in to the message to give room for the BVLL 

	// Generate the NPDU 
	// This is a broadcast message and does not need to be directed at any specific device. 
	ret = GenerateNPDU( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length, 0xFFFF );
	// ret = BACnetAPI.CreateNPDU ( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length, 0xFFFF );
	if( ret <= 0 ) {
		printf( "Error: Could not generate the NPDU request" ); 
		return; 
	}
	buffer_length += ret ; 

	ret = BACnetAPI.WhoHas( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length, NULL, new BACnetPrimitiveCharSTRING( WHOHAS_NAME ) ) ;
	if( ret <= 0 ) {
		printf( "Error: Could not generate the WhoHas request" ); 
		return; 
	}
	buffer_length += ret ; 

	SendMessage( buffer, buffer_length, true) ;
}

void SendReadPropertyMessage( USHORT objectType, UINT objectID, UINT deviceID, USHORT propertyID )
{
	printf( "\n\n----------------------------------------------\n");
	printf( "Read property message\n"); 
	printf( "\tInvokeID: %d\n", BACnetAPI.GetInvokeID() ); 

	int		buffer_length = 0 ;
	BYTE	buffer[ MAXIMUM_BACNET_MESSAGE ] ;
	memset( buffer, 0 , MAXIMUM_BACNET_MESSAGE ); 	

	UINT ret = 0 ; 
	buffer_length = 4 ; // Start 4 bytes in to the message to give room for the BVLL 

	// Generate the object ID 
	ObjectIdentifier.Set( objectType, objectID ) ; 

	// Generate the NPDU 
	/*
	BYTE destinationAddress[10] ; 
	int destinationAddressLength = swsutilities::getnerateDNet( (char *) destinationAddress, 10, deviceID ); 
	

	ret = BACnetAPI.CreateNPDU ( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length,
									DEVICE_NETWORK, destinationAddress, destinationAddressLength,
									true, true );
	*/
	ret = GenerateNPDU( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length, deviceID, DEVICE_NETWORK, true );
	if( ret <= 0 ) {
		printf( "Error: Could not generate the NPDU request" ); 
		return; 
	}
	buffer_length += ret ; 

				

	// Generate the read property message. 
	ret = BACnetAPI.ReadProperty ( 
							buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length,
							&ObjectIdentifier,
							propertyID );
	if( ret <= 0 ) {
		printf( "Error: Could not generate the ReadProperty request" ); 
		return; 
	}
	buffer_length += ret ; 

	SendMessage( buffer, buffer_length) ;
}

void SendReadPropertyMultipleMessage( USHORT objectType, UINT objectID, UINT deviceID, USHORT propertyID ) {

	printf( "\n\n----------------------------------------------\n");
	printf( "Read Property Multiple Message\n" );
	printf( "\tInvokeID: %d\n", BACnetAPI.GetInvokeID() ); 

	int		buffer_length = 0 ;
	BYTE	buffer[ MAXIMUM_BACNET_MESSAGE ] ;
	memset( buffer, 0 , MAXIMUM_BACNET_MESSAGE ); 	

	UINT ret = 0 ; 
	buffer_length = 4 ; // Start 4 bytes in to the message to give room for the BVLL 

	// Generate the object ID 
	ObjectIdentifier.Set( objectType, objectID ) ; 

	// Generate the NPDU 
	ret = GenerateNPDU( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length, deviceID, DEVICE_NETWORK, true );
	if( ret <= 0 ) {
		printf( "Error: Could not generate the NPDU request" ); 
		return; 
	}
	buffer_length += ret ; 

	// Generate the read property message. 
	ReadPropertyMultipleMap map;
	map[ ObjectIdentifier ].push_back( propertyID ) ;

	// Generate the ReadPropertyMultiple 
	ret = BACnetAPI.ReadPropertyMultiple( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length, map ); 
	if( ret <= 0 ) {
		printf( "Error: Could not generate the ReadProperty request" ); 
		return; 
	}
	buffer_length += ret ; 

	SendMessage( buffer, buffer_length) ;
}

void SendReadPropertyMultipleCustomMessage( USHORT objectType, UINT objectID, UINT deviceID) {

	printf( "\n\n----------------------------------------------\n");
	printf( "Read Property Multiple Message\n" );
	printf( "\tInvokeID: %d\n", BACnetAPI.GetInvokeID() ); 

	int		buffer_length = 0 ;
	BYTE	buffer[ MAXIMUM_BACNET_MESSAGE ] ;
	memset( buffer, 0 , MAXIMUM_BACNET_MESSAGE ); 	

	UINT ret = 0 ; 
	buffer_length = 4 ; // Start 4 bytes in to the message to give room for the BVLL 

	// Generate the object ID 
	ObjectIdentifier.Set( objectType, objectID ) ; 

	// Generate the NPDU 
	ret = GenerateNPDU( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length, deviceID, DEVICE_NETWORK, true );
	if( ret <= 0 ) {
		printf( "Error: Could not generate the NPDU request" ); 
		return; 
	}
	buffer_length += ret ; 

	// Generate the read property message. 
	ReadPropertyMultipleMap map;
	map[ ObjectIdentifier ].push_back( BACnetPropertyIdentifier::status_flags ) ;
	map[ ObjectIdentifier ].push_back( BACnetPropertyIdentifier::reliability ) ;
	map[ ObjectIdentifier ].push_back( BACnetPropertyIdentifier::present_value ) ;
	map[ ObjectIdentifier ].push_back( BACnetPropertyIdentifier::object_name ) ;

	// This property propbably will not exist on the server and should produce an error. 
	map[ ObjectIdentifier ].push_back( BACnetPropertyIdentifier::file_size ) ;

	// Search for an 2nd device that does not exist. 
	BACnetPrimitiveObjectIdentifier ObjectIdentifier2; 
	ObjectIdentifier2.Set( objectType, 123456789 ) ; 
	map[ ObjectIdentifier2 ].push_back( BACnetPropertyIdentifier::present_value ) ;

	// Generate the ReadPropertyMultiple 
	ret = BACnetAPI.ReadPropertyMultiple( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length, map ); 
	if( ret <= 0 ) {
		printf( "Error: Could not generate the ReadProperty request" ); 
		return; 
	}
	buffer_length += ret ; 

	SendMessage( buffer, buffer_length) ;
}

void SendWritePropertyMessage( USHORT objectType, UINT objectID, UINT deviceID, float value) 
{
	printf( "\n\n----------------------------------------------\n");
	printf( "Write property message\n"); 
	printf( "\tInvokeID: %d\n", BACnetAPI.GetInvokeID() ); 

	int		buffer_length = 0 ;
	BYTE	buffer[ MAXIMUM_BACNET_MESSAGE ] ;
	memset( buffer, 0 , MAXIMUM_BACNET_MESSAGE ); 	

	UINT ret = 0 ; 
	buffer_length = 4 ; // Start 4 bytes in to the message to give room for the BVLL 

	ObjectIdentifier.Set( objectType, objectID ) ; 

	// Generate the NPDU 
	ret = GenerateNPDU( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length, deviceID, DEVICE_NETWORK, true );
	if( ret <= 0 ) {
		printf( "Error: Could not generate the NPDU request" ); 
		return; 
	}
	buffer_length += ret ; 

	// Genearte the value 
	// Add 1.2f to the value everytime 
	BACnetAbstractSyntaxType propertyValue( new BACnetPrimitiveREAL( value), BACnetApplicationDatatypes::Real ); 

	// Generate the read property message. 
	ret = BACnetAPI.WriteProperty (
							buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length,
							&ObjectIdentifier,
							BACnetPropertyIdentifier::present_value, &propertyValue, WRITE_PRIORITY );
	if( ret <= 0 ) {
		printf( "Error: Could not generate the WriteProperty request" ); 
		return ; 
	}
	buffer_length += ret ; 

	SendMessage( buffer, buffer_length) ;
}


void SendWritePropertyMultipleMessage() {
	
	printf( "\n\n----------------------------------------------\n");
	printf( "Write property Multiple message\n"); 
	printf( "\tInvokeID: %d\n", BACnetAPI.GetInvokeID() ); 

	// printf("Start ObjectBase=[%d]\n", CBACnetBase::GetObjectCount() ); 

	// Start by creating the WritePropertyMap 
	WritePropertyMultipleMap writePropertyMap;

	// The writePropertyMap contains a map of BACnetPrimitiveObjectIdentifier * and BACnetPropertyValue 
	// In this example we will be adding two different writes in to the writePropertyMap
	
	// First value 
	// --------------------------------	
	BACnetPrimitiveObjectIdentifier * objectIdentifier = new BACnetPrimitiveObjectIdentifier( WRITE_OBJECT_TYPE, WRITE_OBJECT_ID ) ; 
	if( objectIdentifier == NULL ) {
		printf("Error: Not enugh memory"); 
		return ; 
	}
	BACnetAbstractSyntaxType * pAbstractSyntaxType = new BACnetAbstractSyntaxType( new BACnetPrimitiveREAL( fWriteValue ), BACnetApplicationDatatypes::Real ) ; 
	BACnetPropertyValue * pPropertyValue = new BACnetPropertyValue(	BACnetPropertyIdentifier::present_value, 0, pAbstractSyntaxType, WRITE_PRIORITY );
	if( pPropertyValue == NULL ) {
		printf("Error: Not enugh memory"); 
		return ; 
	}
	SAFE_DELETE( pAbstractSyntaxType ); 

	writePropertyMap[ objectIdentifier ].push_back( pPropertyValue ) ;
	SAFE_DELETE( objectIdentifier ); 
	
	// Incurment the write value. 
	fWriteValue += 1.2f;	
	
	// Second value 
	// --------------------------------
	objectIdentifier = new BACnetPrimitiveObjectIdentifier( WRITE_OBJECT_TYPE, WRITE_OBJECT_ID ) ; 
	if( objectIdentifier == NULL ) {
		printf("Error: Not enugh memory"); 
		return ; 
	}
	pAbstractSyntaxType = new BACnetAbstractSyntaxType( new BACnetPrimitiveREAL( fWriteValue ), BACnetApplicationDatatypes::Real );
	pPropertyValue = new BACnetPropertyValue( BACnetPropertyIdentifier::present_value, 0, pAbstractSyntaxType, WRITE_PRIORITY );
	if( pPropertyValue == NULL ) {
		printf("Error: Not enugh memory"); 
		return ; 
	}
	SAFE_DELETE( pAbstractSyntaxType ); 

	writePropertyMap[ objectIdentifier ].push_back( pPropertyValue ) ;
	SAFE_DELETE( objectIdentifier ); 
	


	// printf("ObjectBase=[%d]\n", CBACnetBase::GetObjectCount() ); 



	int		buffer_length = 0 ;
	BYTE	buffer[ MAXIMUM_BACNET_MESSAGE ] ;
	memset( buffer, 0 , MAXIMUM_BACNET_MESSAGE ); 	

	UINT ret = 0 ; 
	buffer_length = 4 ; // Start 4 bytes in to the message to give room for the BVLL 
		
	// Generate the NPDU 
	ret = GenerateNPDU( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length, DEVICE_ID, DEVICE_NETWORK, true );
	if( ret <= 0 ) {
		printf( "Error: Could not generate the NPDU request" ); 
		return; 
	}
	buffer_length += ret ; 

	// printf("ObjectBase=[%d]\n", CBACnetBase::GetObjectCount() ); 

	// Generate the read property message. 
	ret = BACnetAPI.WritePropertyMultiple( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length, writePropertyMap );	
	if( ret <= 0 ) {
		printf( "Error: Could not generate the WriteProperty request" ); 
		return ; 
	}
	buffer_length += ret ; 

	// printf("ObjectBase=[%d]\n", CBACnetBase::GetObjectCount() ); 


	// Clean up 
	for( WritePropertyMultipleMap::iterator itr = writePropertyMap.begin() ; itr != writePropertyMap.end(); itr++ ) {
		for( std::vector< BACnetPropertyValue * >::iterator propertyValue_irt = (*itr).second.begin(); propertyValue_irt != (*itr).second.end() ; propertyValue_irt++ ) {
			SAFE_DELETE( (*propertyValue_irt) ); 
		}
		(*itr).second.clear(); 
	}
	writePropertyMap.clear(); 

	// printf("End ObjectBase=[%d]\n", CBACnetBase::GetObjectCount() ); 

	
	
	// ToDo: sending the message causes a memory leak 
	// it pobably has to do with the decode of this message. 

	SendMessage( buffer, buffer_length) ;
}

void SendSubscribeCOVMessage(  USHORT objectType, UINT objectID, UINT deviceID ) {
	
	printf( "\n\n----------------------------------------------\n");
	printf( "Subscribe COV message\n" );

	int		buffer_length = 0 ;
	BYTE	buffer[ MAXIMUM_BACNET_MESSAGE ] ;
	memset( buffer, 0 , MAXIMUM_BACNET_MESSAGE ); 	


	UINT ret = 0 ;
	buffer_length = 4 ;  // Start 4 bytes in to the message to give room for the BVLL

	ObjectIdentifier.Set( objectType, objectID ) ;
	ret = GenerateNPDU( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length, deviceID, DEVICE_NETWORK, true );
	if( ret <= 0 ) {
		printf( "Error: Could not generate the NPDU request" ) ;
		return ;
	}
	buffer_length += ret ;

	// Generate the parameters needed to make a subscription
	unsigned int subscriberProcessIdentifier = 1 ;	// The unique id for this subscription
	bool issueConfirmedNotification = false ;		// False = Receive UnconfirmedCOVNotifications, True = Receive ConfirmedCOVNotifications
	unsigned int lifetime = 60 ;					// How long the subscription will last in seconds.  If set to 0, it will be infinite subscription
				

	ret = BACnetAPI.SubscribeCOV( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length,
							      subscriberProcessIdentifier, &ObjectIdentifier, issueConfirmedNotification, lifetime ) ;
	if( ret <= 0 ) {
		printf( "Error: Could not generate the SubscribeCOV request" ) ;
		return ;
	}
	buffer_length += ret ;

	SendMessage( buffer, buffer_length) ;
}

void SendWhoIsRouterToNetwork( USHORT network) {

	printf( "\n\n----------------------------------------------\n");
	printf( "Subscribe COV message\n" );

	int		buffer_length = 0 ;
	BYTE	buffer[ MAXIMUM_BACNET_MESSAGE ] ;
	memset( buffer, 0 , MAXIMUM_BACNET_MESSAGE ); 	
	
	UINT ret = 0 ;
	buffer_length = 4 ;  // Start 4 bytes in to the message to give room for the BVLL


	// Generate the NPDU
	CBACnetAPINPDU npdu;
	npdu.SetDestinationNetwork( network ); 
	npdu.SetNetworkLayerMessage( (BYTE) CBACnetAPINPDU::Who_Is_Router_To_Network ); 
	ret = npdu.Encode( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length );
	if( ret <= 0 ) {
		printf( "Error: Could not generate the NPDU request" ) ;
		return ;
	}
	buffer_length += ret ;

	// ??? 
	
	SendMessage( buffer, buffer_length) ;
}

USHORT GenerateNPDU( BYTE * buffer, USHORT maxLength, UINT deviceID, USHORT deviceNetwork, bool expectingReply )
{
	
	BYTE destinationAddress[10] ; 									
	int destinationAddressLength = CBACnetAPINPDU::GenerateAddress( (char *) destinationAddress, 10, deviceID ); 
	
	CBACnetAPINPDU npdu;
	if( deviceNetwork != 0 ) {
		npdu.SetDestination( 0xFF, deviceNetwork, destinationAddress, destinationAddressLength);
	}
	npdu.SetExpectingReply( expectingReply ); 
	return npdu.Encode( buffer, maxLength); 
}

void SendForeignDeviceRegistration( ) {

	// Do stuff. 
	printf( "\n\n----------------------------------------------\n");
	printf( "Send Foreign Device Registration\n" );

	int		buffer_length = 0 ;
	BYTE	buffer[ MAXIMUM_BACNET_MESSAGE ] ;
	memset( buffer, 0 , MAXIMUM_BACNET_MESSAGE ); 	

	// Add the BVLL header for BACnet IP messages 
	BACnetVirtualLinkLayer bvll ;

	// Set the Foreign Device Registration
	// Time to live in == 1hr (60 sec * 60 mins * 1hr ) 
	if( ! bvll.SetRegisterForeignDevice( 60*60*1 ) ) {
		printf( "Error: BVLL Set Register Foreign Device failed\n" );
		return ; 
	}
	
	// Encode the BVLL message 
	buffer_length = bvll.Encode( buffer, MAXIMUM_BACNET_MESSAGE ) ;
	if( buffer_length <= 0 ) {
		printf( "Error: BVLL encode failed. \n" );
		return ; 
	}
	
	// Send the message 
	printf( "Sending request, to [%s], size [%d]\n\n", DEVICE_IPADDRESS, buffer_length );
	USHORT ret = udp.SendMessage( DEVICE_IPADDRESS, BACNET_PORT,  buffer, buffer_length ); 
	if( ret <= 0 ) {
		printf( "Error: Could not send message. \n" );
		return ; 
	}	
}

void SendReadPropertyMessageObjectList( UINT deviceID ) {

	printf( "\n\n----------------------------------------------\n");
	printf( "Send Read Property Message for the object list\n" );

	// Create a buffer for the BACnet Message. 
	int		buffer_length = 0 ;
	BYTE	buffer[ MAXIMUM_BACNET_MESSAGE ] ;
	memset( buffer, 0 , MAXIMUM_BACNET_MESSAGE ); 	

	// Set up the Object Identifier
	ObjectIdentifier.Set( BACnetObjectType::device, deviceID ) ; 
	
	// Generate the request for the object count
	// ---------------------------------------------------------------------------

	// Generate the NPDU 
	UINT ret = 0 ; 
	buffer_length = 4 ; // Start 4 bytes in to the message to give room for the BVLL 
	ret = GenerateNPDU( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length, deviceID, DEVICE_NETWORK, true );
	if( ret <= 0 ) {
		printf( "Error: Could not generate the NPDU request" ); 
		return; 
	}
	buffer_length += ret ; 

	// Generate the read property message. 
	ret = BACnetAPI.ReadProperty ( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length, &ObjectIdentifier, BACnetPropertyIdentifier::object_list, true, 0 );
	if( ret <= 0 ) {
		printf( "Error: Could not generate the ReadProperty request" ); 
		return; 
	}
	buffer_length += ret ; 

	SendMessage( buffer, buffer_length) ;

	// Wait for a response and send the next message when ready. 
	// ---------------------------------------------------------------------------

	// Set up some buffers to hold the BACnet messages
	memset( buffer, 0 , MAXIMUM_BACNET_MESSAGE ); 

	BYTE	fromIPAddress[ MAX_IP_ADDRESS_LEN ] ; 
	memset( fromIPAddress, 0 , MAX_IP_ADDRESS_LEN ); 
	std::string ipaddress = DEVICE_IPADDRESS ;
	
	// Create a timeout 
	time_t last_message = time(0); 
	time_t timeout		= 3 ; 

	UINT object_count  = 0 ; 
	UINT object_offset = 0 ; 

	// Loop until user quits. 
	while ( last_message + timeout > time(0) ) 
	{
		// Get a message from the UDP socket on the BACnet port. 
		buffer_length = udp.GetMessage( (char *) buffer, MAXIMUM_BACNET_MESSAGE, (char *) fromIPAddress ) ; 
		if( buffer_length < 0 ) {
			continue; // No message yet. 
		}
		// Got a message on our BACnet port
		printf( "FYI. Got message, from [%s], size [%d]\n", fromIPAddress, buffer_length  );
				
		// Decode this message
		if( BACnetResponce.Decode( buffer+4, buffer_length-4 ) <= 0  ) {			
			// Error could not decode message. 
			printf( "Can not decode the message\n\n" );
			continue ; 
		}

		printf( "Incomming: \n" ); 
		PrintMessageAsXML( buffer, buffer_length ); 
		last_message = time(0) ; 

		// Check the response 
		unsigned char pdutype = BACnetResponce.GetPDUType() ; 
		if( pdutype != BACnetPDU::complexACK ) {
			continue; 
		}
		CBACnetComplexAckPDU * pComplexAckPDU =  BACnetResponce.GetPDUComplexAck();
		if( pComplexAckPDU == NULL ) {
			continue; 
		}		
		if( pComplexAckPDU->service_ACK_choice != BACnetServiceAckChoice::readProperty ) {
			continue; 
		}
		BACnetReadPropertyAck * pReadPropertyAck = ( BACnetReadPropertyAck *) pComplexAckPDU->service_ACK ;
		if( pReadPropertyAck == NULL ) {
			continue; 
		}
		if( ! pReadPropertyAck->isValid() ) {
			continue; 
		}		

		printf( "ReadProperty. originalInvokeID=[%d] ObjectType=[%d], ObjectInstance=[%d], PropertyIdentifier=[%d] ",
			pComplexAckPDU->original_invokeID, 
			pReadPropertyAck->objectIdentifier.GetObjectType(),
			pReadPropertyAck->objectIdentifier.GetInstanceNumber(),
			pReadPropertyAck->propertyIdentifier.Get() ); 

		switch( pReadPropertyAck->propertyValue.GetDataType() ) 
		{
			case BACnetApplicationDatatypes::UnsignedInteger: 
			{
				// Got the total object count 
				BACnetPrimitiveUINT * pPrimitiveUINT = pReadPropertyAck->propertyValue.GetBACnetPrimitiveUINT();
				if( pPrimitiveUINT != NULL ) {
					printf( "\nThis device has a total of %d objects.\n", pPrimitiveUINT->Get() ); 
					object_count = pPrimitiveUINT->Get() ; 
				}
			}
			case BACnetApplicationDatatypes::ObjectIdentifier: 
			{
				// Got one of the objects 
				BACnetPrimitiveObjectIdentifier * pPrimitiveObjectIdentifier = pReadPropertyAck->propertyValue.GetBACnetPrimitiveObjectIdentifier();
				if( pPrimitiveObjectIdentifier != NULL ) {
					printf( "\nFound object. Type=%d, ID=%d.\n", pPrimitiveObjectIdentifier->GetObjectType(),  pPrimitiveObjectIdentifier->GetInstanceNumber() ); 
				}

				// Send the next request. 
				// ---------------------------------------------------------------------------
				if( object_offset < object_count ) {
					object_offset++;


					// Generate the NPDU 
					ret = 0 ; 
					buffer_length = 4 ; // Start 4 bytes in to the message to give room for the BVLL 
					ret = GenerateNPDU( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length, deviceID, DEVICE_NETWORK, true );
					if( ret <= 0 ) {
						printf( "Error: Could not generate the NPDU request" ); 
						return; 
					}
					buffer_length += ret ; 

					// Generate the read property message. 
					ret = BACnetAPI.ReadProperty ( buffer+buffer_length, MAXIMUM_BACNET_MESSAGE-buffer_length, &ObjectIdentifier, BACnetPropertyIdentifier::object_list, true, object_offset );
					if( ret <= 0 ) {
						printf( "Error: Could not generate the ReadProperty request" ); 
						return; 
					}
					buffer_length += ret ; 

					SendMessage( buffer, buffer_length) ;
				}

			}
		}
	}
}